//读写csv文件所需
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <random>
#include<algorithm>
std::ofstream outFile; 
void row_to_string_vector(std::vector<std::string> &record, const std::string& line, char delimiter)
{
    int linepos=0;
    char c;
    int linemax=line.length();
    std::string curstring;
    record.clear();
    while(linepos<linemax)
    {
        c = line[linepos];
        if(isdigit(c)||c=='.'||c=='-'){
            curstring+=c;
        }
        else if(c==delimiter&&curstring.size()){
            record.push_back(curstring);
            curstring="";
        }
        ++linepos;
    }
    if(curstring.size())
        record.push_back(curstring);
    return;
}
void read_csv(const std::string filename, std::vector<std::vector<float>> &out_vector, bool squeze)
{
    std::vector<std::string> row_element;
    std::string line;
    std::ifstream in(filename,std::ios::in);     
    if (in.fail())  { std::cout << "File not found" <<std::endl; return ; }     
    while(getline(in, line)  && in.good() )
    {   
        if(squeze==true)
        {
            squeze=false;
            
        }           
        else
        {
            std::vector<float> temp_vector;
            row_to_string_vector(row_element, line, ',');  //把line里的单元格数字字符提取出来，“,”为单元格分隔符
            for(int i=0, leng=row_element.size(); i<leng; i++){
                temp_vector.push_back(atof( row_element[i].c_str() ));
            } 
            out_vector.push_back(temp_vector);
        }        
    }
    in.close();
    return ;
}

void write_csv(const std::string filename,const std::vector<std::vector<float>> &in_vector)
{
    outFile.open(filename, std::ios::ate|std::ios::out);
    // 5000*128
    std::cout<<"外围维度为"<<in_vector.size()<<std::endl;
    std::cout<<"内围维度为"<<in_vector[0].size()<<std::endl;
    for(int i=0;i<in_vector.size();i++){
        
        for(int j=0;j<in_vector[0].size();j++){
            outFile << in_vector[i][j] << ',';
        }
        outFile <<std::endl;
    }   
      
}
void write_rtk_recording(const std::string filename,const std::vector<std::vector<float>> &in_vector)
{
    outFile.open(filename, std::ios::ate|std::ios::out);
    // 5000*128
    outFile<<"x"<<','<<"y"<<','<<"theta"<<','<<"kappa"<<','<<"v"<<','<<"s"<<','<<"a"<<std::endl;
    std::cout<<"外围维度为"<<in_vector.size()<<std::endl;
    std::cout<<"内围维度为"<<in_vector[0].size()<<std::endl;
    for(int i=0;i<in_vector.size();i++){
        
        for(int j=0;j<in_vector[0].size();j++){
            outFile << in_vector[i][j] << ',';
        }
        outFile <<std::endl;
    }   
}
void resample(const std::vector<std::vector<float>> &in_vector ,
    std::vector<std::vector<float>> &out_vector,const float delta_s)
{
    float temp_s =0 ;
    std::vector<float> temp_vector;
    std::vector<float> last_temp_vector;
    temp_vector = in_vector[0];
    out_vector.push_back(temp_vector);
    last_temp_vector = temp_vector;
    for(int i=1;i<in_vector.size();i++)
    {   
        temp_s = in_vector[i][5]-last_temp_vector[5];
        if(temp_s>delta_s)
        {
            std::cout<<temp_s<<">"<<delta_s<<std::endl;
            temp_vector = in_vector[i];
            out_vector.push_back(temp_vector);
            last_temp_vector = temp_vector;
        }
    }    
}
void preprocess(const std::string in_filename,const std::string out_filename,const float delta_s)
{
    std::vector<std::vector<float>> temp_out_vector;

    read_csv(in_filename,temp_out_vector,true);

    std::vector<std::vector<float> > temp_in_vector(temp_out_vector.size()
        , std::vector<float>(7, 0));  
    float sum_s = 0;  
    float kappa = 0;
    for(int i=0;i<temp_out_vector.size();i++) //i is row index
    {

        temp_in_vector[i][0] = ( temp_out_vector[i][7]+temp_out_vector[i][8] )*0.5;//x
        temp_in_vector[i][1] = ( temp_out_vector[i][10]+temp_out_vector[i][11] )*0.5;//y
        if(i>=1)
        {
            float delta_x = temp_in_vector[i][0] - temp_in_vector[i-1][0];
            float delta_y = temp_in_vector[i][1] - temp_in_vector[i-1][1];
            float delta_s = std::pow( delta_x*delta_x+delta_y*delta_y,0.5 );
            float delta_theta = temp_out_vector[i][9]- temp_out_vector[i-1][9];//yaw
            
            if (delta_s>0.001)
            {
                kappa=delta_theta/delta_s;
            }
            else
            {
                /* code */
            }
                             
            sum_s+=delta_s;
            
            
        }
        temp_in_vector[i][2] =temp_out_vector[i][9];//theta
        temp_in_vector[i][3] =kappa*3.14/180;//kappa
        float temp_vx = ( temp_out_vector[i][3]+temp_out_vector[i][4] )*0.5;
        float temp_vy = ( temp_out_vector[i][5]+temp_out_vector[i][6] )*0.5;
        float temp_v =std::pow(temp_vx*temp_vx+temp_vy*temp_vy,0.5);
        temp_in_vector[i][4] =temp_v;//v->3 4 5 6 行
        temp_in_vector[i][5] =sum_s;//s
        float temp_a = std::pow( std::pow(temp_out_vector[i][1],2)+std::pow(temp_out_vector[i][2],2), 0.5 );
        temp_in_vector[i][6] =temp_out_vector[i][1];//a->1 2行   
    }
    temp_in_vector[0][3] =temp_in_vector[1][3];//first point kappa is equal to second point
    std::vector<std::vector<float>> resample_vector;
    resample(temp_in_vector,resample_vector,delta_s);
    float resample_kappa=0;
    for(int i=0;i<resample_vector.size();i++)
    {
        if(i>=1)
        {
            float resample_delta_theta = resample_vector[i][2]-resample_vector[i-1][2];
            float resample_delta_s = resample_vector[i][5]-resample_vector[i-1][5];
            if (resample_delta_s>0.001)
            {
                resample_kappa=resample_delta_theta/resample_delta_s;
            }          
            else
            {
                
            }
            resample_vector[i][3] = resample_kappa*3.14/180;
        }     
    }
    resample_vector[0][3] = resample_vector[1][3];
    std::cout<<"resample sucess"<<std::endl;
    write_rtk_recording(out_filename,resample_vector);
    std::cout<<"rtk sucess"<<std::endl;
}

int main()
{
    preprocess("LastRun.csv","rtk_recording.csv",0.2);
    /*std::vector<std::vector<float>> test_vector;
    std::vector<float> temp0{1,2,3};
    test_vector.push_back(temp0);
    std::vector<float> temp1{4,5,6};
    test_vector.push_back(temp1);
    write_csv("test.csv",test_vector);
    test_vector.clear();
    read_csv("test.csv",test_vector);
    std::cout<<test_vector[0][0]<<test_vector[0][1]<<std::endl;*/

}